#include "stdafx.h"
#include "CollisionDetection.h"

#include "CollisionData.h"
#include "PhysicsPrimitives.h"


#define CT mphys::Contact

void mphys::sphereSphereCollision(Sphere & s1, Sphere & s2, CollisionData & data)
{
	XMVECTOR pos1 = XMLoadFloat3( &s1.Position );
	XMVECTOR pos2 = XMLoadFloat3( &s2.Position );

	XMVECTOR dir = XMVectorSubtract( pos1, pos2 );
	

	float centersDistSq = XMVectorGetX( XMVector3LengthSq( dir ) );

	if ( centersDistSq > (s1.Radius + s2.Radius) * (s1.Radius + s2.Radius) )
		return;

	float centersDist = XMVectorGetX( XMVector3Length( dir ) );

	XMVECTOR normal = XMVectorMultiply( dir, XMVectorReplicate( 1.0f / centersDist ) );
	float penetration = s1.Radius + s2.Radius - centersDist;
	XMVECTOR point = XMVectorMultiplyAdd( normal, XMVectorReplicate( s2.Radius - penetration * 0.5f ), pos2 ); // normal is directed from s2 to s1

	Contact contact;
	contact.Body[0] = &s1;
	contact.Body[1] = &s2;
	XMStoreFloat3( &contact.Normal, normal );
	XMStoreFloat3( &contact.Point, point );
	contact.Penetration = penetration;
	contact.Restitution = 0.9f;

	data.getContacts().push_back( contact );
}

void mphys::planeSphereCollision( Plane & p1, Sphere & s1, CollisionData & data )
{
	XMVECTOR spherePos = XMLoadFloat3( &s1.Position );
	XMVECTOR planeNormal = XMLoadFloat3( &p1.Normal );

	float sphereToPlaneDist = XMVectorGetX( XMVector3Dot( planeNormal, spherePos ) ) - p1.Offset - s1.Radius;

	if ( sphereToPlaneDist > 0 )
		return;

	Contact contact;
	contact.Body[0] = &s1;
	contact.Body[1] = &p1;
	XMStoreFloat3( &contact.Normal, planeNormal );
	XMStoreFloat3( &contact.Point, XMVectorSubtract( // point = spherePos - planeNormal * (sphereToPlaneDist + s1.Radius)
		spherePos,
		XMVectorMultiply( planeNormal, XMVectorReplicate( sphereToPlaneDist + s1.Radius ) )
		) );
	contact.Penetration = -sphereToPlaneDist;
	contact.Restitution = 0.4f;

	data.getContacts().push_back( contact );
}

void mphys::boxPlaneCollision( Box & b1, Plane & p1, CollisionData & data )
{
	XMVECTOR planeNormal = XMLoadFloat3( &p1.Normal );
	float planeOffset = p1.Offset;

	//XMVECTOR boxPoint;
	//float pointDistance;

	float hX = b1.HalfSizeX;
	float hY = b1.HalfSizeY;
	float hZ = b1.HalfSizeZ;

	XMVECTOR boxPoints[8] =
	{
		XMVectorSet(-hX, -hY, -hZ, 0.0f),
		XMVectorSet(-hX, -hY,  hZ, 0.0f),
		XMVectorSet(-hX,  hY, -hZ, 0.0f),
		XMVectorSet(-hX,  hY,  hZ, 0.0f),
		XMVectorSet( hX, -hY, -hZ, 0.0f),
		XMVectorSet( hX, -hY, +hZ, 0.0f),
		XMVectorSet( hX,  hY, -hZ, 0.0f),
		XMVectorSet( hX,  hY, +hZ, 0.0f)
	};

	XMMATRIX boxWorldMatrix = XMLoadFloat4x4(&b1.WorldMatrix);

	for (size_t i = 0; i < 8 ; i++)
	{
		boxPoints[i] = XMVector3Transform(boxPoints[i], boxWorldMatrix);
	}
	
	for (size_t i = 0; i < 8 ; i++)
	{
		float pointDistance = XMVectorGetX(XMVector3Dot(boxPoints[i], planeNormal));
		if(pointDistance <= planeOffset)
		{
			Contact contact;
			contact.Body[0] = &b1;
			contact.Body[1] = &p1;
			XMStoreFloat3( &contact.Normal, planeNormal );
			contact.Penetration = planeOffset - pointDistance;
			contact.Restitution = 0.3f;
			XMStoreFloat3( &contact.Point, XMVectorMultiplyAdd(planeNormal, XMVectorReplicate(contact.Penetration), boxPoints[i]) );

			data.getContacts().push_back( contact );
		}
	}
}

void mphys::boxSphereCollision( Box & b1, Sphere & s1, CollisionData & data )
{
	XMVECTOR sphereCenter = XMLoadFloat3( &s1.Position );
	XMVECTOR tmp;
	XMMATRIX boxWorldMatrix = XMLoadFloat4x4( &b1.WorldMatrix );
	XMMATRIX boxWorldMatrixInv = XMMatrixInverse( &tmp, boxWorldMatrix );

	XMVECTOR relCenter = XMVector3Transform( sphereCenter, boxWorldMatrixInv );

	float relCenterX = XMVectorGetX( relCenter );
	float relCenterY = XMVectorGetY( relCenter );
	float relCenterZ = XMVectorGetZ( relCenter );

	// Early out check to see if we can exclude the contact
	if (abs(relCenterX) - s1.Radius > b1.HalfSizeX ||
		abs(relCenterY) - s1.Radius > b1.HalfSizeY ||
		abs(relCenterZ) - s1.Radius > b1.HalfSizeZ)
	{
		return;
	}

	XMVECTOR closestPt = XMVectorZero();
	float dist;

	// Clamp each coordinate to the box.
	dist = relCenterX;
	if (dist > b1.HalfSizeX) dist = b1.HalfSizeX;
	if (dist < -b1.HalfSizeX) dist = -b1.HalfSizeX;
	closestPt.m128_f32[0] = dist;

	dist = relCenterY;
	if (dist > b1.HalfSizeY) dist = b1.HalfSizeY;
	if (dist < -b1.HalfSizeY) dist = -b1.HalfSizeY;
	closestPt.m128_f32[1] = dist;

	dist = relCenterZ;
	if (dist > b1.HalfSizeZ) dist = b1.HalfSizeZ;
	if (dist < -b1.HalfSizeZ) dist = -b1.HalfSizeZ;
	closestPt.m128_f32[2] = dist;

	// Check we're in contact
	dist = XMVectorGetX( XMVector3LengthSq ( XMVectorSubtract( closestPt, relCenter ) ) );
	if (dist > s1.Radius * s1.Radius) return;

	// Compile the contact
	XMVECTOR closestPtWorld = XMVector3Transform( closestPt, boxWorldMatrix );

	Contact contact;
	contact.Body[0] = &b1;
	contact.Body[1] = &s1;
	XMStoreFloat3( &contact.Normal, XMVector3Normalize( XMVectorSubtract( closestPtWorld, sphereCenter ) ) );
	XMStoreFloat3( &contact.Point, closestPtWorld );
	contact.Penetration = s1.Radius - sqrtf(dist);
	contact.Restitution = 0.2f;


	data.getContacts().push_back( contact );
}

// helper methods for box-box collision

static float transformToAxis( const mphys::Box & box, XMVECTOR axis )
{
	XMVECTOR halfSize = XMVectorSet( box.HalfSizeX, box.HalfSizeY, box.HalfSizeZ, 0.0f );
	XMVECTOR axesProjections = XMVectorSet(
		XMVectorGetX( XMVector3Dot( axis, XMLoadFloat3( &box.Axes[0] ) ) ),
		XMVectorGetX( XMVector3Dot( axis, XMLoadFloat3( &box.Axes[1] ) ) ),
		XMVectorGetX( XMVector3Dot( axis, XMLoadFloat3( &box.Axes[2] ) ) ),
		0.0f);

	return XMVectorGetX( XMVector3Dot( halfSize, XMVectorAbs( axesProjections ) ) );
}

/*
 * This function checks if the two boxes overlap
 * along the given axis, returning the amount of overlap.
 * The final parameter toCentre
 * is used to pass in the vector between the boxes center
 * points, to avoid having to recalculate it each time.
 */
static float penetrationOnAxis( const mphys::Box & one, const mphys::Box & two, XMVECTOR axis, XMVECTOR toCenter )
{
	// Project the half-size of one onto axis
	float oneProject = transformToAxis( one, axis );
	float twoProject = transformToAxis( two, axis );
	
	// Project this onto the axis
	float distance = fabsf( XMVectorGetX( XMVector3Dot( toCenter, axis ) ) );

	// Return the overlap (i.e. positive indicates
	// overlap, negative indicates separation).
	return oneProject + twoProject - distance;
}

static bool tryAxis( const mphys::Box & one, const mphys::Box & two, XMVECTOR axis, XMVECTOR toCenter, size_t index,
	/* These values may be updated */ float & smallestPenetration, unsigned & smallestCase )
{
	// Make sure we have a normalized axis, and don't check almost parallel axes
	if ( XMVectorGetX( XMVector3LengthSq( axis ) ) < 0.0001 ) return true;
	axis = XMVector3Normalize( axis );

	float penetration = penetrationOnAxis(one, two, axis, toCenter);

	if (penetration < 0) return false;
	if (penetration < smallestPenetration) {
		smallestPenetration = penetration;
		smallestCase = index;
	}
	return true;
}

void fillPointFaceBoxBox( mphys::Box & one, mphys::Box & two, XMVECTOR toCenter, mphys::CollisionData & data, size_t best, float pen )
{
	// This method is called when we know that a vertex from
	// box two is in contact with box one.

	// We know which axis the collision is on (i.e. best),
	// but we need to work out which of the two faces on
	// this axis.
	XMVECTOR normal = XMLoadFloat3( &one.Axes[best] );
	if( XMVectorGetX( XMVector3Dot( normal, toCenter ) ) > 0.0f )
	{
		normal = XMVectorNegate( normal );
	}

	// Work out which vertex of box two we're colliding with.
	// Using toCenter doesn't work!
	XMVECTOR vertex = XMVectorSet( two.HalfSizeX, two.HalfSizeY, two.HalfSizeZ, 0.0f );
	if( XMVectorGetX( XMVector3Dot( XMLoadFloat3( &two.Axes[0] ), normal) ) > 0 ) vertex.m128_f32[0] = -vertex.m128_f32[0];
	if( XMVectorGetX( XMVector3Dot( XMLoadFloat3( &two.Axes[1] ), normal) ) > 0 ) vertex.m128_f32[1] = -vertex.m128_f32[1];
	if( XMVectorGetX( XMVector3Dot( XMLoadFloat3( &two.Axes[2] ), normal) ) > 0 ) vertex.m128_f32[2] = -vertex.m128_f32[2];

	mphys::Contact contact;
	contact.Body[0] = &one;
	contact.Body[1] = &two;
	XMStoreFloat3( &contact.Normal, normal );
	XMStoreFloat3( &contact.Point, XMVector3Transform( vertex, XMLoadFloat4x4( &two.WorldMatrix ) ) );
	contact.Penetration = pen;
	contact.Restitution = 0.2f;

	data.getContacts().push_back( contact );
}

static XMVECTOR contactPoint( XMVECTOR pOne, XMVECTOR dOne, float oneSize, XMVECTOR pTwo, const XMVECTOR & dTwo, float twoSize,
	// If this is true, and the contact point is outside
	// the edge (in the case of an edge-face contact) then
	// we use one's midpoint, otherwise we use two's.
	bool useOne)
{
	XMVECTOR toSt, cOne, cTwo;
	float dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
	float denom, mua, mub;

	smOne = XMVectorGetX( XMVector3LengthSq( dOne ) );
	smTwo = XMVectorGetX( XMVector3LengthSq( dTwo ) );
	dpOneTwo = XMVectorGetX( XMVector3Dot( dTwo, dOne ) );

	toSt = pOne - pTwo;
	dpStaOne = XMVectorGetX( XMVector3Dot( dOne, toSt ) );
	dpStaTwo = XMVectorGetX( XMVector3Dot( dTwo, toSt ) );

	denom = smOne * smTwo - dpOneTwo * dpOneTwo;

	// Zero denominator indicates parallel lines
	if ( fabsf( denom ) < 0.0001f )
		return useOne ? pOne : pTwo;

	mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom;
	mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom;

	// If either of the edges has the nearest point out
	// of bounds, then the edges aren't crossed, we have
	// an edge-face contact. Our point is on the edge, which
	// we know from the useOne parameter.
	if (mua > oneSize ||
		mua < -oneSize ||
		mub > twoSize ||
		mub < -twoSize)
	{
		return useOne ? pOne : pTwo;
	}
	else
	{
		cOne = XMVectorMultiplyAdd( dOne, XMVectorReplicate( mua ), pOne );
		cTwo = XMVectorMultiplyAdd( dTwo, XMVectorReplicate( mub ), pTwo );

		return XMVectorAdd(
			XMVectorMultiply( cOne, XMVectorReplicate( 0.5f ) ),
			XMVectorMultiply( cTwo, XMVectorReplicate( 0.5f ) )
		);
	}
}

#define CHECK_OVERLAP(axis, index) if (!tryAxis(b1, b2, (axis), toCenter, (index), pen, best)) return false;


bool mphys::boxBoxCollision( Box & b1, Box & b2, CollisionData & data )
{
	//if (!IntersectionTests::boxAndBox(one, two)) return 0;

	// Find the vector between the two centers
	XMVECTOR toCenter = XMVectorSubtract( XMLoadFloat3( &b2.Position ), XMLoadFloat3( &b1.Position ) );

	// We start assuming there is no contact
	float pen = (std::numeric_limits<float>::max)();
	size_t best = 0xffffff;

	XMVECTOR b1Axes[] = { XMLoadFloat3( &b1.Axes[0] ), XMLoadFloat3( &b1.Axes[1] ), XMLoadFloat3( &b1.Axes[2] ) };
	XMVECTOR b2Axes[] = { XMLoadFloat3( &b2.Axes[0] ), XMLoadFloat3( &b2.Axes[1] ), XMLoadFloat3( &b2.Axes[2] ) };

	// Now we check each axes, returning if it gives us
	// a separating axis, and keeping track of the axis with
	// the smallest penetration otherwise.
	CHECK_OVERLAP( b1Axes[0], 0 );
	CHECK_OVERLAP( b1Axes[1], 1 );
	CHECK_OVERLAP( b1Axes[2], 2 );

	CHECK_OVERLAP( b2Axes[0], 3 );
	CHECK_OVERLAP( b2Axes[1], 4 );
	CHECK_OVERLAP( b2Axes[2], 5 );

	// Store the best axis-major, in case we run into almost
	// parallel edge collisions later
	size_t bestSingleAxis = best;

	if (!tryAxis(b1, b2, XMVector3Cross( b1Axes[0], b2Axes[0] ), toCenter, 6, pen, best)) return false;
	if (!tryAxis(b1, b2, XMVector3Cross( b1Axes[0], b2Axes[1] ), toCenter, 7, pen, best)) return false;
	if (!tryAxis(b1, b2, XMVector3Cross( b1Axes[0], b2Axes[2] ), toCenter, 8, pen, best)) return false;

	//CHECK_OVERLAP( XMVector3Cross( b1Axes[0], b2Axes[0] ), 6 );
	//CHECK_OVERLAP( XMVector3Cross( b1Axes[0], b2Axes[1] ), 7 );
	//CHECK_OVERLAP( XMVector3Cross( b1Axes[0], b2Axes[2] ), 8 );
	CHECK_OVERLAP( XMVector3Cross( b1Axes[1], b2Axes[0] ), 9 );
	CHECK_OVERLAP( XMVector3Cross( b1Axes[1], b2Axes[1] ), 10 );
	CHECK_OVERLAP( XMVector3Cross( b1Axes[1], b2Axes[2] ), 11 );
	CHECK_OVERLAP( XMVector3Cross( b1Axes[2], b2Axes[0] ), 12 );
	CHECK_OVERLAP( XMVector3Cross( b1Axes[2], b2Axes[1] ), 13 );
	CHECK_OVERLAP( XMVector3Cross( b1Axes[2], b2Axes[2] ), 14 );

	// Make sure we have got a result.
	assert( best != 0xffffff );

	// We now know there's a collision, and we know which
	// of the axes gave the smallest penetration. We now
	// can deal with it in different ways depending on
	// the case.
	if ( best < 3 )
	{
		// We've got a vertex of box two on a face of box one.
		fillPointFaceBoxBox( b1, b2, toCenter, data, best, pen );
		return true;
	}
	else if ( best < 6 )
	{
		// We have got a vertex of box one on a face of box two.
		// We use the same algorithm as above, but swap around
		// one and two (and therefore also the vector between their
		// centers).
		fillPointFaceBoxBox( b1, b2, XMVectorNegate( toCenter ), data, best-3, pen );
		return true;
	}
	else
	{
		// We have got an edge-edge contact. Find out which axes
		best -= 6;
		unsigned oneAxisIndex = best / 3;
		unsigned twoAxisIndex = best % 3;
		XMVECTOR oneAxis = b1Axes[oneAxisIndex];
		XMVECTOR twoAxis = b2Axes[twoAxisIndex];
		XMVECTOR axis = XMVector3Cross( oneAxis, twoAxis );
		axis = XMVector3Normalize( axis );

		// The axis should point from box one to box two.
		if( XMVectorGetX( XMVector3Dot( axis, toCenter ) ) > 0.0f ) axis = XMVectorNegate( axis );

		// We have the axes, but not the edges: each axis has 4 edges parallel
		// to it, we need to find which of the 4 for each object. We do
		// that by finding the point in the center of the edge. We know
		// its component in the direction of the box's collision axis is zero
		// (its a mid-point) and we determine which of the extremes in each
		// of the other axes is closest.
		XMVECTOR b1HalfSize = XMVectorSet( b1.HalfSizeX, b1.HalfSizeY, b1.HalfSizeZ, 0.0f );
		XMVECTOR b2HalfSize = XMVectorSet( b2.HalfSizeX, b2.HalfSizeY, b2.HalfSizeZ, 0.0f );

		XMVECTOR ptOnOneEdge = b1HalfSize;
		XMVECTOR ptOnTwoEdge = b2HalfSize;
		for ( size_t i = 0; i < 3; i++ )
		{
			if ( i == oneAxisIndex ) ptOnOneEdge.m128_f32[i] = 0;
			else if ( XMVectorGetX( XMVector3Dot( b1Axes[i], axis ) ) > 0 ) ptOnOneEdge.m128_f32[i] = -ptOnOneEdge.m128_f32[i];

			if (i == twoAxisIndex) ptOnTwoEdge.m128_f32[i] = 0;
			else if ( XMVectorGetX( XMVector3Dot( b2Axes[i], axis ) ) < 0 ) ptOnTwoEdge.m128_f32[i] = -ptOnTwoEdge.m128_f32[i];
		}

		// Move them into world coordinates (they are already oriented
		// correctly, since they have been derived from the axes).
		ptOnOneEdge = XMVector3Transform( ptOnOneEdge, XMLoadFloat4x4( &b1.WorldMatrix ) );
		ptOnTwoEdge = XMVector3Transform( ptOnTwoEdge, XMLoadFloat4x4( &b2.WorldMatrix ) );

		// So we have a point and a direction for the colliding edges.
		// We need to find out point of closest approach of the two
		// line-segments.
		XMVECTOR vertex = contactPoint(
			ptOnOneEdge, oneAxis, b1HalfSize.m128_f32[oneAxisIndex],
			ptOnTwoEdge, twoAxis, b2HalfSize.m128_f32[twoAxisIndex],
			bestSingleAxis > 2
			);

		mphys::Contact contact;
		contact.Body[0] = &b1;
		contact.Body[1] = &b2;
		XMStoreFloat3( &contact.Normal, axis );
		XMStoreFloat3( &contact.Point, vertex );
		contact.Penetration = pen;
		contact.Restitution = 0.2f;

		data.getContacts().push_back( contact );
	}
	return 0;
}
